/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package dawnland03.data.map.algorithm.astar;

import java.util.*;

import dawnland03.data.map.model.Matrix;
import dawnland03.data.map.model.MatrixCell;
import dawnland03.data.map.model.MatrixDirection;
import dawnland03.model.entity.Entity;

/**
 * @author Petru Obreja (obrejap@yahoo.com)
 */
public class AStarAlgorithm {

    private MatrixOperations matrixOperations;

    public AStarAlgorithm(MatrixOperations matrixOperations) {
        this.matrixOperations = matrixOperations;
    }

    /*
    function A*(start,goal)
        closedset := the empty set                 % The set of nodes already evaluated.
        openset := set containing the initial node % The set of tentative nodes to be evaluated.
        g_score[start] := 0                        % Distance from start along optimal path.
        h_score[start] := heuristic_estimate_of_distance(start, goal)
        f_score[start] := h_score[start]           % Estimated total distance from start to goal through y.
        while openset is not empty
            x := the node in openset having the lowest f_score[] value
            if x = goal
                return reconstruct_path(came_from,goal)
            remove x from openset
            add x to closedset
            foreach y in neighbor_nodes(x)
                if y in closedset
                    continue
                tentative_g_score := g_score[x] + dist_between(x,y)
                tentative_is_better := false
                if y not in openset
                    add y to openset
                    h_score[y] := heuristic_estimate_of_distance(y, goal)
                    tentative_is_better := true
                elseif tentative_g_score < g_score[y]
                    tentative_is_better := true
                if tentative_is_better = true
                    came_from[y] := x
                    g_score[y] := tentative_g_score
                    f_score[y] := g_score[y] + h_score[y]
        return failure

    function reconstruct_path(came_from,current_node)
        if came_from[current_node] is set
            p = reconstruct_path(came_from,came_from[current_node])
            return (p + current_node)
        else
            return the empty path
    */

    /* In the standard terminology used when talking about A*,
     g(n) represents the cost of the path from the starting point to any vertex n,
     and h(n) represents the heuristic estimated cost from vertex n to the goal.*/

    public List<MatrixCell> getClosestPath(Entity entity, Matrix matrix, int xStart, int yStart, int xFinish, int yFinish) {
        List<MatrixCell> closestPath = null;
        MatrixCell startNode = matrix.getMatrixCell(xStart, yStart);
        MatrixCell finishNode = matrix.getMatrixCell(xFinish, yFinish);
        Set<MatrixCell> closedSet = new HashSet<MatrixCell>(); //The set of nodes already evaluated.
//        Set<AlgorithmCell> openset = new TreeSet<AlgorithmCell>(); //The set of tentative nodes to be evaluated.
        List<AlgorithmCell> openList = new LinkedList<AlgorithmCell>();
        openList.add(createAStarAlgorithmCell(startNode, 0, heuristicEstimateOfDistance(entity, matrix, startNode, finishNode)));
        while (openList.size() > 0) {
            AlgorithmCell currentNode = openList.iterator().next();
            if (currentNode.getNode().equals(finishNode)) {
                //computePath
                closestPath = getPath(currentNode);
                break;
            }
            openList.remove(currentNode);
            closedSet.add(currentNode.getNode());
            Map<MatrixDirection, MatrixCell> neighbours = matrix.getNeighbours(currentNode.getNode());
            for (MatrixCell neighbour : neighbours.values()) {
                if (closedSet.contains(neighbour)) {
                    continue;
                }
                double tentativeGScore = currentNode.getGScore() + getCostDistanceBetweenTwoNodes(entity, matrix, currentNode.getNode(), neighbour);
                boolean tentativeIsBetter = false;
                double hScoreNeighbour = heuristicEstimateOfDistance(entity, matrix, neighbour, finishNode);
                AlgorithmCell neighbourCell = getAlgorithmCell(openList, neighbour);
                if (neighbourCell == null) {
                    neighbourCell = createAStarAlgorithmCell(neighbour, tentativeGScore, hScoreNeighbour);
                    openList.add(neighbourCell);
                    Collections.sort(openList);
                    tentativeIsBetter = true;
                } else if (tentativeGScore < neighbourCell.getGScore()) {
                    tentativeIsBetter = true;
                }
                if (tentativeIsBetter) {
                    neighbourCell.setPreviousCellInPath(currentNode);
                    neighbourCell.setGScore(tentativeGScore);
                    neighbourCell.setHScore(hScoreNeighbour);
                }
            }
        }
        return closestPath;
    }

    private AlgorithmCell createAStarAlgorithmCell(MatrixCell node, double gValue, double hValue) {
        AlgorithmCell algorithmCell = new AlgorithmCell(node);
        algorithmCell.setGScore(gValue);
        algorithmCell.setHScore(hValue);
        return algorithmCell;
    }

    private double heuristicEstimateOfDistance(Entity entity, Matrix matrix, MatrixCell x, MatrixCell y) {
        //T0910061.1 compute average cost dynamically
        //multiply this by matrix average cost
        return matrixOperations.computeDistanceBetweenTwoNodes(entity, matrix, x, y);
    }

    private double getCostDistanceBetweenTwoNodes(Entity entity, Matrix matrix, MatrixCell x, MatrixCell y) {
        return matrixOperations.computeCostBetweenTwoNodes(entity, matrix, x, y);
    }

    private AlgorithmCell getAlgorithmCell(List<AlgorithmCell> set, MatrixCell node) {
        for (AlgorithmCell cell : set) {
            if (cell.getNode().equals(node)) {
                return cell;
            }
        }
        return null;
    }

    private List<MatrixCell> getPath(AlgorithmCell finishCell) {
        List<MatrixCell> path = new LinkedList<MatrixCell>();
        AlgorithmCell currentCell = finishCell;
        do {
            path.add(currentCell.getNode());
            currentCell = currentCell.getPreviousCellInPath();
        } while (currentCell != null);
        Collections.reverse(path);
        return path;
    }
}
